#define BT_RX 20  // ESP32-C3的接收引脚
#define BT_TX 21  // ESP32-C3的发送引脚
HardwareSerial bluetoothSerial = Serial1;
// 定义电机控制引脚
const int FL_PIN = 3;  // 前左轮
const int FR_PIN = 5;  // 前右轮
const int BL_PIN = 6;  // 后左轮
const int BR_PIN = 9;  // 后右轮

// 定义最大线速度和角速度
const float V_MAX = 0.3667;  // 最大线速度 (m/s)
const float OMEGA_MAX = 2.9336;  // 最大角速度 (rad/s)

// 定义机器人尺寸
const float L = 0.25;  // 机器人宽度 (m)
const float W = 0.25;  // 机器人长度 (m)

void setup() {
  Serial.begin(115200); // 主串口用于调试
  bluetoothSerial.begin(9600, SERIAL_8N1, BT_RX, BT_TX); // 蓝牙串口
  // 进入命令模式并配置 HC-05
  delay(1000); // 给蓝牙模块一点时间启动
  bluetoothSerial.print("AT+ROLE?\r\n");
  bluetoothSerial.print("AT+ROLE=1\r\n");
  bluetoothSerial.print("AT+NAME=HC05\r\n");
  bluetoothSerial.print("AT+PIN=1234\r\n");
  bluetoothSerial.print("AT+AUTOCON=1\r\n");
  bluetoothSerial.print("AT+SAVE\r\n");

  // 设置电机控制引脚为输出
  pinMode(FL_PIN, OUTPUT);
  pinMode(FR_PIN, OUTPUT);
  pinMode(BL_PIN, OUTPUT);
  pinMode(BR_PIN, OUTPUT);
}

void loop() {
  // 读取遥感输入值
  int X = 0;
  int Y = 0;

  // 从串口读取X轴和Y轴的输入值
  if (Serial.available() >= 2) {
    X = Serial.read() ;  // 读取X轴值，范围-128到128
    Y = Serial.read() ;  // 读取Y轴值，范围-128到128
  }

  // 归一化输入值
  float X_norm = X / 128.0;
  float Y_norm = Y / 128.0;

  // 计算线速度和角速度
  float V_x = X_norm * V_MAX;
  float V_y = Y_norm * V_MAX;
  float omega = (Y_norm - X_norm) * OMEGA_MAX;

  // 计算每个轮子的速度
  float V_FL = V_x - V_y + omega * (L / 2 + W / 2);
  float V_FR = V_x + V_y - omega * (L / 2 + W / 2);
  float V_BL = V_x + V_y + omega * (L / 2 + W / 2);
  float V_BR = V_x - V_y - omega * (L / 2 + W / 2);

  // 将速度转换为PWM值和方向
  setMotorSpeed(FL_IN1_PIN, FL_IN2_PIN, FL_PWM_PIN, V_FL);
  setMotorSpeed(FR_IN1_PIN, FR_IN2_PIN, FR_PWM_PIN, V_FR);
  setMotorSpeed(BL_IN1_PIN, BL_IN2_PIN, BL_PWM_PIN, V_BL);
  setMotorSpeed(BR_IN1_PIN, BR_IN2_PIN, BR_PWM_PIN, V_BR);

  delay(10);  // 延迟10毫秒
}

void setMotorSpeed(int in1_pin, int in2_pin, int pwm_pin, float speed) {
  // 将速度转换为PWM值
  int pwm_value = map(abs(speed), 0, V_MAX, 0, 255);

  // 设置方向
  if (speed >= 0) {
    digitalWrite(in1_pin, HIGH);  // 正转
    digitalWrite(in2_pin, LOW);
  } else {
    digitalWrite(in1_pin, LOW);   // 反转
    digitalWrite(in2_pin, HIGH);
  }

  // 设置PWM值
  analogWrite(pwm_pin, pwm_value);
}